#pragma once
#include "elisa/elisa.h"
#include <iostream>
#include <sstream>
#include <algorithm>
#include <string>
#include <vector>
#include <gaol/gaol>

#include "Modeler.h"

#ifdef ELISA_PERFORMER
#include "elisa_chrono.h"
#endif

using namespace std;
using namespace elisa;

class Goughstewart : public Modeler
{
private:
	Model _model;

	RealVar x1;
	RealVar x2;
	RealVar x3;
	RealVar y1;
	RealVar y2;
	RealVar y3;
	RealVar z1;
	RealVar z2;
	RealVar z3;
	RealConstraint c1;
	RealConstraint c2;
	RealConstraint c3;
	RealConstraint c4;
	RealConstraint c5;
	RealConstraint c6;
	RealConstraint c7;
	RealConstraint c8;
	RealConstraint c9;

public:
	Goughstewart()
		: x1(Interval(-2,5.57), "x1"),
		  x2(Interval(-6.25,1.3), "x2"),
		  x3(Interval(-5.39,0.7), "x3"),
		  y1(Interval(-5.57,2.7), "y1"),
		  y2(Interval(-6.25,2.7), "y2"),
		  y3(Interval(-5.39,3.11), "y3"),
		  z1(Interval(0,5.57), "z1"),
		  z2(Interval(-2,6.25), "z2"),
		  z3(Interval(-3.61,5.39), "z3"),
		  c1(pow(x1,2) + pow(y1,2) + pow(z1,2) == 31),
		  c2(pow(x2,2) + pow(y2,2) + pow(z2,2) == 39),
		  c3(pow(x3,2) + pow(y3,2) + pow(z3,2) == 29),
		  c4(x1*x2 + y1*y2 + z1*z2 + 6*x1 - 6*x2 == 51),
		  c5(x1*x3 + y1*y3 + z1*z3 + 7*x1 - 2*y1 - 7*x3 + 2*y3 == 50),
		  c6(x2*x3 + y2*y3 + z1*z3 + x2 - 2*y2 - x3 + 2*y3 == 34),
		  c7(-12*x1 + 15*y1 - 10*x2 - 25*y2 + 18*x3 + 18*y3 == -32),
		  c8(-14*x1 + 35*y1 - 36*x2 - 45*y2 + 30*x3 + 18*y3 == 8),
		  c9(2*x1 + 2*y1 - 14*x2 - 2*y2 + 8*x3 - y3 == 20),
		  _model()
	{
	}

	elisa::Model & model()
	{
		_model = c1 && c2 && c3 && c4 && c5 && c6 && c7 && c8 && c9;
		return _model;
	}

	void print()
	{
		cout << '\t' << x1.getName() << " = " << x1.domain() << endl;
		cout << '\t' << x2.getName() << " = " << x2.domain() << endl;
		cout << '\t' << x3.getName() << " = " << x3.domain() << endl;
		cout << '\t' << y1.getName() << " = " << y1.domain() << endl;
		cout << '\t' << y2.getName() << " = " << y2.domain() << endl;
		cout << '\t' << y3.getName() << " = " << y3.domain() << endl;
		cout << '\t' << z1.getName() << " = " << z1.domain() << endl;
		cout << '\t' << z2.getName() << " = " << z2.domain() << endl;
		cout << '\t' << z3.getName() << " = " << z3.domain() << endl;
	}

	RealVar & varX()
	{
		return x1;
	}
	RealVar & varY()
	{
		return x2;
	}
};
